#include "clkWorld.h"

clkWorld::clkWorld()
{

}

clkWorld::~clkWorld()
{
	destroy();
}

void clkWorld::init(float a_cWidth, float a_cHeight)
{
	m_grid.init(a_cWidth,a_cHeight);
}

void clkWorld::destroy()
{
	m_conList.release();
	m_objList.release();
	m_objQueue.release();
	m_objPool.release();
	m_grid.destroy();
}

void clkWorld::cullBox(V2DF a_min,V2DF a_max)
{
	m_cull.m_min = a_min;
	m_cull.m_max = a_max;
}

void clkWorld::updatePhysics(float a_time, int a_iter)
{
	clkObject* obj = 0;
	for(int i = 0; i < m_objList.size(); i++)
	{
		obj = m_objList[i];

		if(!obj->m_isAsleep && !obj->m_isStationary)
		{
			clamp(obj->m_lnFrc,0.0f,obj->m_mxLnFrc);
			clamp(obj->m_agFrc,-obj->m_mxAgFrc,obj->m_mxAgFrc);

			calcPhysics(
				obj->m_pos,obj->m_rot,obj->m_lnVel,obj->m_agVel,
				obj->m_lnFrc,obj->m_agFrc,obj->m_lnDamp,obj->m_agDamp,
				obj->m_mass,obj->m_iner,a_time);

			obj->update();
		}

		if(overlapAABB(obj->m_aabb,m_cull))
			m_grid.queueObj(m_objList[i]);
	}
}

void clkWorld::generateContacts(float a_time, int a_iter)
{
	//create temp object value for use
	clkCell* cell = 0;
	clkObject* objA = 0;
	clkObject* objB = 0;

	//remove difference
	int removeDiff = 0;

	//loop through all the cells
	while(m_grid.amountCell() != 0)
	{
		//remove the front cell to test
		m_grid.obtainCell(&cell);

		//if the cell has 1 object then delete the cell and continue
		if(cell->m_objList.size() <= 1)
		{
			cell->m_memIndex -= removeDiff;
			cell->m_objList.clear();
			m_grid.removeCell(&cell);
			++removeDiff;
			continue;
		}

		//loop through objects and find contacts
		while(cell->m_objList.size() >= 1)
		{
			//grab the front object to test against all others
			objA = cell->m_objList.removeFront();

			if(!objA->m_isCollidable)
				continue;

			//loop through the rest of the list to test
			for(int i = 0; i < cell->m_objList.size(); i++)
			{
				//grab the current indexed object
				objB = cell->m_objList[i];

				if(!objB->m_isCollidable)
					continue;

				if(!overlapAABB(objA->m_aabb,objB->m_aabb))
					continue;

				//test objA against objB
				V2DF mtvAxis; float mtvMag = 0;
				if(satCollision(
					objA->m_vertR,objA->m_vertRN,
					objB->m_vertR,objB->m_vertRN,
					&mtvAxis,&mtvMag))
				{
					//create the contacts if objects have collided
					int idx = m_conList.size();
					m_conList.insertBack(clkContact());
					m_conList[idx].m_conObj[0] = objA;
					m_conList[idx].m_conObj[1] = objB;
					m_conList[idx].m_mtvAxis   = mtvAxis;
					m_conList[idx].m_mtvMag    = mtvMag;
				}
			}
		}

		//delete the cell once done
		cell->m_memIndex -= removeDiff;
		cell->m_objList.clear();
		m_grid.removeCell(&cell);
		++removeDiff;
	}
}

void clkWorld::resolveContacts(float a_time, int a_iter)
{
	//temp variables for use
	clkContact* contact = 0;
	clkObject* objA = 0;
	clkObject* objB = 0;

	//loop and resolve the contacts
	while(m_conList.size() != 0)
	{
		//get the front contact
		contact = &m_conList.removeFront();
		
		//get the objects
		objA = contact->m_conObj[0];
		objB = contact->m_conObj[1];

		float impMul = 400.0f * a_time;
		V2DF  impDir = (contact->m_mtvAxis * (contact->m_mtvMag));

		if(objA->m_isStationary)
		{
			//resolve interpenetration for objB
			objB->m_pos   -= impDir;
			objB->m_lnVel -= impDir * impMul;
		}
		else if(objB->m_isStationary)
		{
			//resolve interpenetration for objA
			objA->m_pos   += impDir;
			objA->m_lnVel += impDir * impMul;
		}
		else
		{
			//resolve interpenetration for objA
			//objA->m_pos   += (impDir * 0.5f);
			objA->m_lnVel += (impDir * 0.5f) * impMul;

			//resolve interpenetration for objB
			//objB->m_pos   -= (impDir * 0.5f);
			objB->m_lnVel -= (impDir * 0.5f) * impMul;
		}
	}
}

clkObject* clkWorld::createObject(clkObjectDef& a_objDef)
{
	if(a_objDef.m_type == SHAPE_NONE) return 0;

	int blk = 0, idx = 0;
	clkObject* obj = m_objPool.newData(&blk,&idx);

	obj->m_objBlk  = blk;
	obj->m_objIdx  = idx;
	
	obj->m_radius  = a_objDef.m_radius;
	obj->m_width   = a_objDef.m_width;
	obj->m_height  = a_objDef.m_height;
	obj->m_pos     = a_objDef.m_pos;
	obj->m_rot     = a_objDef.m_rot;
	obj->m_mass    = a_objDef.m_mass;
	obj->m_iner    = a_objDef.m_iner;
	obj->m_lnVel   = a_objDef.m_lnVel;
	obj->m_agVel   = a_objDef.m_agVel;
	obj->m_lnDamp  = a_objDef.m_lnDamp;
	obj->m_agDamp  = a_objDef.m_agDamp;
	obj->m_lnFrc   = a_objDef.m_lnFrc;
	obj->m_agFrc   = a_objDef.m_agFrc;
	obj->m_mxLnFrc = a_objDef.m_mxLnFrc;
	obj->m_mxAgFrc = a_objDef.m_mxAgFrc;

	obj->m_isAsleep     = a_objDef.m_isAsleep;
	obj->m_isCollidable = a_objDef.m_isCollidable;
	obj->m_isStationary = a_objDef.m_isStationary;

	obj->create();

	m_objList.insertBack(obj);

	return obj;
}

void clkWorld::destroyObject(clkObject* a_object)
{
	m_objList.removeData(a_object);
	m_objPool.deleteData(&a_object);
}

void clkWorld::pullObject(clkObject* a_object)
{
	m_objList.insertBack(m_objQueue.removeData(a_object));
}

void clkWorld::waitObject(clkObject* a_object)
{
	m_objQueue.insertBack(m_objList.removeData(a_object));
}